import serial
import minimalmodbus
import time
import threading
import sys
import struct
import csv
import os
from flask import Flask, jsonify, request, send_file
from flask_cors import CORS
from datetime import datetime

# 全局状态管理
class AppState:
    def __init__(self):
        self.running = False
        self.measuring = False
        self.data = []
        self.current_angle = None
        self.current_distance = None
        self.system_status = "未连接"
        self.serial_port = None
        self.stepper = None
        self.motor = None
        self.sensor = None
        self.collector = None
        self.csv_file = None  # 动态生成CSV路径
        self.total_move_distance = 0
        self.lead = 5  # 步进电机导程(mm)
        self.config = {
            "port": "COM6",
            "baudrate": 115200,
            "motor_id": 0x01,
            "sensor_address": 4,
            "stepper_id": 2,
            "repeat_times": 50,
            "stepper_cycles": 2,
            "sample_interval": 0.05,
            "measure_mode": "continuous",  # 采集模式：continuous(连续) / fixed_angle(定点)
            "angle_step": 10  # 定点模式角度间隔(度)
        }
        self.lock = threading.Lock()

state = AppState()
app = Flask(__name__)
CORS(app)

# -------------------------- 电机/传感器类 --------------------------
class StepperMotor:
    """步进电机控制类（地址3）"""
    def __init__(self, serial_port, motor_id=3, lock=None, 细分=10000):
        self.ser = serial_port
        self.motor_id = motor_id
        self.细分 = 细分
        self.lock = lock or threading.Lock()
        self.lead = 5

    def _send_modbus_command(self, register, value):
        with self.lock:
            try:
                cmd = bytearray()
                cmd.append(self.motor_id)
                cmd.append(0x06)
                cmd.extend(register.to_bytes(2, byteorder='big'))
                cmd.extend(value.to_bytes(2, byteorder='big'))
                crc = self._calc_crc(cmd)
                cmd.extend(crc)
                self.ser.reset_input_buffer()
                self.ser.reset_output_buffer()
                self.ser.write(cmd)
                self.ser.flush()
                time.sleep(0.01)
                resp = self.ser.read(8)
                return len(resp) == 8
            except Exception as e:
                print(f"[Stepper] 发送命令失败: {e}")
                return False

    def _calc_crc(self, data):
        crc = 0xFFFF
        for byte in data:
            crc ^= byte
            for _ in range(8):
                if crc & 0x0001:
                    crc >>= 1
                    crc ^= 0xA001
                else:
                    crc >>= 1
        return crc.to_bytes(2, byteorder='little')

    def rotate_cycles(self, cycles, speed=100, accel=10, decel=10, timeout=300):
        print(f"开始控制步进电机旋转 {cycles} 圈...")
        target_position = int(cycles * self.细分)
        if not self._send_modbus_command(0x0191, 0x0028):
            print("设置PR0电流失败")
            return 0
        if not self._send_modbus_command(0x6200, 0x0041):
            print("设置PR0模式失败")
            return 0
        position_high = (target_position >> 16) & 0xFFFF
        position_low = target_position & 0xFFFF
        if not self._send_modbus_command(0x6201, position_high):
            print("设置PR0位置高位失败")
            return 0
        if not self._send_modbus_command(0x6202, position_low):
            print("设置PR0位置低位失败")
            return 0
        if not self._send_modbus_command(0x6203, speed):
            print("设置PR0速度失败")
            return 0
        if not self._send_modbus_command(0x6204, accel):
            print("设置PR0加速度失败")
            return 0
        if not self._send_modbus_command(0x6205, decel):
            print("设置PR0减速度失败")
            return 0
        time.sleep(1)
        return abs(cycles * self.lead)

    def rotate_start(self, cycles, speed=50, accel=20, decel=20, timeout=300):
        print(f"步进电机开始旋转 {cycles} 圈...")
        if not self._send_modbus_command(0x6002, 0x0010):
            print("触发PR0运行失败")
            return False
        move_distance = abs(cycles * self.lead)
        with state.lock:
            state.total_move_distance += move_distance
        time.sleep(1)
        return True

    def emergency_stop(self):
        return self._send_modbus_command(0x6002, 0x0040)

class HaitaiMotor:
    """云台电机控制类（支持连续/定点两种模式）"""
    def __init__(self, serial_port, motor_id=0x01, lock=None):
        if serial_port is None:
            raise ValueError("必须提供共享 serial_port 实例")
        self.ser = serial_port
        self.motor_id = motor_id
        self.lock = lock or threading.Lock()
        self.counts_per_circle = 16384
        self.packet_seq = 0x00

    def _crc16_modbus(self, data):
        crc = 0xFFFF
        for byte in data:
            crc ^= byte
            for _ in range(8):
                if crc & 0x0001:
                    crc >>= 1
                    crc ^= 0xA001
                else:
                    crc >>= 1
        return crc & 0xFFFF

    def _pack_frame(self, cmd, data_bytes=b''):
        frame_header = [0x3E, self.packet_seq, self.motor_id, cmd, len(data_bytes)]
        frame_header_bytes = bytes(frame_header)
        crc = self._crc16_modbus(frame_header_bytes + data_bytes)
        crc_bytes = struct.pack('<H', crc)
        full_frame = frame_header_bytes + data_bytes + crc_bytes
        self.packet_seq = (self.packet_seq + 1) & 0xFF
        return full_frame

    def _send_command(self, cmd, data_bytes=b'', read_len=15, post_delay=0.1):
        full_frame = self._pack_frame(cmd, data_bytes)
        with self.lock:
            try:
                self.ser.reset_input_buffer()
                self.ser.reset_output_buffer()
                self.ser.write(full_frame)
                self.ser.flush()
                time.sleep(post_delay)
                resp = self.ser.read(read_len)
                if resp and len(resp) >= 5 and resp[0] == 0x3C:
                    return resp
                else:
                    print(f"[电机] 应答无效: 长度={len(resp) if resp else 0}, 数据={resp.hex() if resp else '无'}")
                    return None
            except Exception as e:
                print(f"[电机] 发送命令失败: {e}")
                raise

    def get_current_angle(self):
        try:
            resp = self._send_command(0x2F, b'', read_len=15, post_delay=0.1)
        except Exception as e:
            print(f"[电机] 读取角度出错: {e}")
            return None
        if resp and len(resp) == 15:
            try:
                single_turn_raw = (resp[5] | (resp[6] << 8)) & 0x3FFF
                single_turn_angle = single_turn_raw * (360.0 / self.counts_per_circle)
                multi_turn_raw = (resp[7] | (resp[8] << 8) | (resp[9] << 16) | (resp[10] << 24))
                if multi_turn_raw & 0x80000000:
                    multi_turn_raw -= 0x100000000
                multi_turn_angle = multi_turn_raw * (360.0 / self.counts_per_circle)
                return multi_turn_angle
            except Exception as e:
                print(f"[电机] 解析角度失败: {e} | 应答数据: {resp.hex()}")
                return None
        else:
            print(f"[电机] 应答长度错误（需15字节）: 实际={len(resp) if resp else 0}")
            return None

    def set_origin(self):
        print("设置当前位置为原点...")
        try:
            resp = self._send_command(0x21, b'', read_len=10, post_delay=0.5)
            if resp and len(resp) >= 10:
                result = resp[7]
                if result == 0x01:
                    print("原点设置成功")
                    return True
                else:
                    print(f"原点设置失败，结果码: 0x{result:02X}")
                    return False
            else:
                print(f"[电机] 设置原点应答无效: {resp.hex() if resp else '无响应'}")
                return False
        except Exception as e:
            print(f"[电机] 设置原点失败: {e}")
            return False

    def move_to_absolute_position(self, target_angle, max_speed=100):
        target_counts = int(target_angle * (self.counts_per_circle / 360.0))
        target_bytes = struct.pack('<I', target_counts)
        speed_bytes = struct.pack('<H', max_speed)
        try:
            self._send_command(0x57, b'\x01' + speed_bytes, post_delay=0.2)
            time.sleep(0.1)
            self._send_command(0x55, target_bytes, post_delay=0.2)
            return True
        except Exception as e:
            print(f"[电机] 绝对值位置控制失败: {e}")
            return False

    def move_to_zero(self, timeout=300):
        print("开始回归零位...")
        if not self.set_origin():
            return False
        if not self.move_to_absolute_position(0, max_speed=200):
            return False
        start_time = time.time()
        while time.time() - start_time < timeout:
            current_angle = self.get_current_angle()
            if current_angle is not None and abs(current_angle) <= 0.1:
                print("已到达零位")
                return True
            time.sleep(0.2)
        print("回零超时")
        return False

    def rotate_360_continuous(self, timeout=300):
        """原有连续旋转模式（360度实时采集）"""
        print("开始连续旋转360度...")
        if not self.move_to_absolute_position(360, max_speed=100):
            return False
        start_time = time.time()
        while time.time() - start_time < timeout:
            current_angle = self.get_current_angle()
            if current_angle is not None:
                normalized_angle = current_angle % 360
                if abs(normalized_angle - 0) <= 0.1:
                    print("已完成360度旋转")
                    return True
            time.sleep(0.2)
        print("旋转超时")
        return False

    def rotate_fixed_angle(self, angle_step, round_number, max_speed=100):
        """新增定点角度模式：按设定间隔旋转并采集（接收轮次参数）"""
        print(f"开始定点角度采集（间隔{angle_step}度，第{round_number}轮）...")
        total_angle = 360
        current_target = 0
        success = True
        while current_target <= total_angle and state.running:
            # 移动到目标角度
            if not self.move_to_absolute_position(current_target, max_speed):
                print(f"移动到{current_target}度失败")
                success = False
                break
            # 稳定1秒后采集数据
            time.sleep(1.0)
            current_angle = self.get_current_angle()
            distance = state.sensor.read_distance()
            if current_angle is not None and distance is not None:
                normalized_angle = current_angle % 360
                current_time = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3]
                with state.lock:
                    # 保存到全局数据
                    state.current_angle = normalized_angle
                    state.current_distance = distance
                    state.data.append((current_time, state.total_move_distance, normalized_angle, distance))
                # 实时打印
                print(f"定点采集 | 轮次{round_number} | 角度: {normalized_angle:.2f}° | 距离: {distance:.5f}mm", end='\r', flush=True)
                # 追加到CSV（传入当前轮次）
                self._append_fixed_angle_data(current_time, state.total_move_distance, normalized_angle, distance, round_number)
            # 计算下一个目标角度
            current_target += angle_step
        print(f"\n第{round_number}轮定点角度采集完成")
        return success

    def _append_fixed_angle_data(self, current_time, move_distance, angle, distance, round_number):
        """定点数据追加到CSV（修复轮次写入）"""
        try:
            file_exists = os.path.isfile(state.csv_file)
            with open(state.csv_file, 'a', newline='', encoding='utf-8') as f:
                writer = csv.writer(f)
                if not file_exists:
                    writer.writerow([
                        "测量时间(精确到毫秒)", "移动距离(mm)", "云台电机旋转角度(°)",
                        "激光位移传感器测量数据(mm)", "采集模式", "测量轮次"
                    ])
                writer.writerow([
                    current_time, round(move_distance, 1), round(angle, 4),
                    round(distance, 5), "定点角度", round_number  # 写入当前轮次
                ])
        except Exception as e:
            print(f"定点数据保存失败: {e}")

    def read_status(self):
        try:
            resp = self._send_command(0x40, b'', read_len=12, post_delay=0.1)
            if resp and len(resp) >= 12:
                voltage = resp[5] * 0.2
                current = resp[6] * 0.03
                temperature = resp[7] * 0.4
                fault_code = resp[8]
                run_mode = resp[9]
                mode_map = {0: "关闭状态", 1: "开环模式", 3: "速度模式", 5: "位置模式"}
                print(f"[状态] 电压: {voltage:.1f}V, 电流: {current:.2f}A, 温度: {temperature:.1f}℃, 故障码: 0x{fault_code:02X}, 运行模式: {mode_map.get(run_mode, '未知')}")
                return {
                    'voltage': voltage,
                    'current': current,
                    'temperature': temperature,
                    'fault_code': fault_code,
                    'run_mode': run_mode
                }
            else:
                print(f"[电机] 读取状态应答无效: {resp.hex() if resp else '无响应'}")
        except Exception as e:
            print(f"[电机] 读取状态失败: {e}")
        return None

    def close_motor(self):
        try:
            self._send_command(0x50, b'', post_delay=0.2)
            print("电机已关闭，进入自由态")
        except Exception as e:
            print(f"[电机] 关闭失败: {e}")

    def close(self):
        self.close_motor()

class LE100485Sensor:
    def __init__(self, serial_port, sensor_address=2, lock=None):
        if serial_port is None:
            raise ValueError("必须提供共享 serial_port 实例")
        self.serial_port = serial_port
        self.lock = lock or threading.Lock()
        self.sensor_address = sensor_address
        self.instrument = minimalmodbus.Instrument(serial_port, slaveaddress=self.sensor_address)
        self.instrument.mode = minimalmodbus.MODE_RTU
        self.instrument.serial = self.serial_port
        self.instrument.close_port_after_each_call = False
        self.instrument.clear_buffers_before_each_transaction = True
        self.instrument.debug = False
        self.distance_register = 0x003B
        self.resolution = 0.001

    def read_distance(self, register_address=None):
        if register_address is None:
            register_address = self.distance_register
        with self.lock:
            try:
                raw_data = self.instrument.read_register(
                    registeraddress=register_address,
                    functioncode=3,
                    signed=False
                )
                return raw_data * self.resolution
            except Exception as e:
                print(f"[Sensor] 读取失败: {e}")
                return None

    def close(self):
        pass

class DataCollector:
    """连续模式数据采集器"""
    def __init__(self, motor, sensor):
        self.motor = motor
        self.sensor = sensor
        self.data = []
        self.running = False
        self.thread = None

    def _collect_loop(self, interval):
        while self.running:
            current_time = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3]
            angle = self.motor.get_current_angle()
            distance = self.sensor.read_distance()
            with state.lock:
                move_distance = state.total_move_distance
            if angle is not None and distance is not None:
                normalized_angle = angle % 360
                self.data.append((current_time, move_distance, normalized_angle, distance))
                with state.lock:
                    state.current_angle = normalized_angle
                    state.current_distance = distance
                    state.data.append((current_time, move_distance, normalized_angle, distance))
                print(f"连续采集 | 时间: {current_time} | 角度: {normalized_angle:.2f}° | 距离: {distance:.5f}mm", end='\r', flush=True)
            time.sleep(interval)

    def start(self, interval=0.05):
        if self.running:
            return
        self.data = []
        self.running = True
        self.thread = threading.Thread(target=self._collect_loop, args=(interval,), daemon=True)
        self.thread.start()

    def stop(self):
        self.running = False
        if self.thread:
            self.thread.join(timeout=2)

    def append_to_csv(self, round_number):
        try:
            file_exists = os.path.isfile(state.csv_file)
            with open(state.csv_file, 'a', newline='', encoding='utf-8') as f:
                writer = csv.writer(f)
                if not file_exists:
                    writer.writerow([
                        "测量时间(精确到毫秒)", "移动距离(mm)", "云台电机旋转角度(°)",
                        "激光位移传感器测量数据(mm)", "采集模式", "测量轮次"
                    ])
                for row in self.data:
                    writer.writerow(list(row) + ["连续旋转", round_number])
            print(f"\n第{round_number}轮连续数据已追加到 {state.csv_file}，新增 {len(self.data)} 条记录")
            return True
        except Exception as e:
            print(f"连续数据保存失败: {e}")
            return False

# -------------------------- 测量任务逻辑 --------------------------
def measurement_task():
    try:
        with state.lock:
            state.system_status = "初始化中"
        state.motor.read_status()
        if not state.motor.move_to_zero():
            with state.lock:
                state.system_status = "云台电机无法回归零位"
            return
        init_move_distance = state.stepper.rotate_cycles(state.config["stepper_cycles"])
        if init_move_distance <= 0:
            with state.lock:
                state.system_status = "步进电机初始化失败"
            return
        with state.lock:
            state.total_move_distance = init_move_distance

        for i in range(state.config["repeat_times"]):
            with state.lock:
                if not state.running:
                    break
                state.system_status = f"测量中 (第{i+1}/{state.config['repeat_times']}轮)"
            print(f"\n===== 开始第 {i+1}/{state.config['repeat_times']} 轮测量 =====")
            
            # 步进电机旋转
            if not state.stepper.rotate_start(state.config["stepper_cycles"]):
                with state.lock:
                    state.system_status = f"第{i+1}轮：步进电机旋转失败"
                return

            # 根据模式执行不同采集逻辑
            if state.config["measure_mode"] == "continuous":
                # 连续模式
                collector = DataCollector(state.motor, state.sensor)
                with state.lock:
                    state.collector = collector
                collector.start(interval=state.config["sample_interval"])
                if not state.motor.rotate_360_continuous():
                    collector.stop()
                    with state.lock:
                        state.system_status = f"第{i+1}轮：云台旋转失败"
                    return
                collector.stop()
                collector.append_to_csv(i+1)
                with state.lock:
                    state.collector = None
            else:
                # 定点角度模式（传入当前轮次 i+1）
                if not state.motor.rotate_fixed_angle(state.config["angle_step"], i+1):
                    with state.lock:
                        state.system_status = f"第{i+1}轮：定点采集失败"
                    return

            # 每轮采集完成后回归零位
            state.motor.move_to_zero()

        # 所有轮次完成后关闭电机
        state.motor.close_motor()
        with state.lock:
            state.system_status = "测量完成"
        print("\n所有测量任务完成！")
    except Exception as e:
        error_msg = str(e)
        with state.lock:
            state.system_status = f"出错: {error_msg}"
        print(f"任务执行出错: {e}")
    finally:
        with state.lock:
            state.measuring = False

# -------------------------- API接口 --------------------------
@app.route('/api/connect', methods=['POST'])
def connect_device():
    try:
        config = request.json
        if not config:
            return jsonify({"status": "error", "message": "请提供配置参数"}), 400
        with state.lock:
            state.config.update(config)
            # 关闭已有串口连接
            if state.serial_port and state.serial_port.is_open:
                state.serial_port.close()
            # 初始化串口
            state.serial_port = serial.Serial(
                port=state.config["port"],
                baudrate=state.config["baudrate"],
                parity=serial.PARITY_NONE,
                stopbits=serial.STOPBITS_ONE,
                bytesize=serial.EIGHTBITS,
                timeout=1,
                write_timeout=5
            )
            # 启用RS485模式（如果支持）
            try:
                if hasattr(serial, "rs485"):
                    rs485_settings = serial.rs485.RS485Settings(
                        rts_level_for_tx=True,
                        rts_level_for_rx=False,
                        delay_before_tx=0.002,
                        delay_before_rx=0.002
                    )
                    state.serial_port.rs485_mode = rs485_settings
                    print("[API] 已启用RS485模式")
            except Exception as e:
                print(f"[API] 启用RS485模式失败（忽略）: {e}")
            # 初始化设备实例
            serial_lock = threading.Lock()
            state.stepper = StepperMotor(
                serial_port=state.serial_port,
                motor_id=state.config["stepper_id"],
                lock=serial_lock
            )
            state.motor = HaitaiMotor(
                serial_port=state.serial_port,
                motor_id=state.config["motor_id"],
                lock=serial_lock
            )
            state.sensor = LE100485Sensor(
                serial_port=state.serial_port,
                sensor_address=state.config["sensor_address"],
                lock=serial_lock
            )
            state.system_status = "已连接"
        return jsonify({
            "status": "success",
            "message": "设备连接成功",
            "config": state.config
        })
    except serial.SerialException as e:
        msg = f"串口连接失败: {str(e)} | 请检查串口号、USB转485连接和设备供电"
        with state.lock:
            state.system_status = "连接失败"
        return jsonify({"status": "error", "message": msg}), 500
    except Exception as e:
        with state.lock:
            state.system_status = "连接失败"
        return jsonify({"status": "error", "message": f"连接异常: {str(e)}"}), 500

@app.route('/api/set-mode', methods=['POST'])
def set_measure_mode():
    """设置采集模式和角度间隔"""
    try:
        data = request.json
        if "measure_mode" not in data:
            return jsonify({"status": "error", "message": "请指定采集模式"}), 400
        with state.lock:
            state.config["measure_mode"] = data["measure_mode"]
            # 定点模式必须指定角度间隔
            if data["measure_mode"] == "fixed_angle":
                if "angle_step" not in data or not isinstance(data["angle_step"], int) or data["angle_step"] <= 0 or data["angle_step"] > 180:
                    return jsonify({"status": "error", "message": "角度间隔必须为1-180之间的整数"}), 400
                state.config["angle_step"] = data["angle_step"]
        return jsonify({
            "status": "success",
            "message": f"已设置为{data['measure_mode']}模式",
            "config": {
                "measure_mode": state.config["measure_mode"],
                "angle_step": state.config["angle_step"]
            }
        })
    except Exception as e:
        return jsonify({"status": "error", "message": f"设置失败: {str(e)}"}), 500

@app.route('/api/start', methods=['POST'])
def start_measure():
    with state.lock:
        if state.system_status not in ["已连接", "测量完成", "待机"]:
            return jsonify({"status": "error", "message": "请先连接设备"}), 400
        if state.running:
            return jsonify({"status": "error", "message": "测量已在进行中"}), 400
        # 动态生成CSV文件路径（关键修复）
        state.csv_file = f"测量数据_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv"
        state.running = True
        state.measuring = True
        state.data = []
        state.total_move_distance = 0  # 重置移动距离
        # 启动测量线程
        threading.Thread(target=measurement_task, daemon=True).start()
    return jsonify({
        "status": "success",
        "message": "测量已启动",
        "csv_file": state.csv_file
    })

@app.route('/api/stop', methods=['POST'])
def stop_measure():
    with state.lock:
        if not state.running:
            return jsonify({"status": "error", "message": "测量未在进行中"}), 400
        state.running = False
        state.measuring = False
        if state.collector:
            state.collector.stop()
        state.system_status = "待机"
    return jsonify({"status": "success", "message": "测量已停止"})

@app.route('/api/emergency', methods=['POST'])
def emergency_stop():
    with state.lock:
        state.running = False
        state.measuring = False
        if state.collector:
            state.collector.stop()
        if state.stepper:
            state.stepper.emergency_stop()
        state.system_status = "已急停"
    return jsonify({"status": "success", "message": "已执行紧急停止"})

@app.route('/api/reset', methods=['POST'])
def reset_system():
    with state.lock:
        state.running = False
        state.measuring = False
        state.data = []
        state.current_angle = None
        state.current_distance = None
        state.total_move_distance = 0
        if state.collector:
            state.collector.stop()
            state.collector = None
        if state.motor:
            try:
                state.motor.move_to_zero()
            except Exception as e:
                print(f"[API] 复位云台失败: {e}")
        state.system_status = "待机"
    return jsonify({"status": "success", "message": "系统已复位"})

@app.route('/api/status', methods=['GET'])
def get_system_status():
    with state.lock:
        return jsonify({
            "system_status": state.system_status,
            "data_count": len(state.data),
            "current_angle": round(state.current_angle, 4) if state.current_angle is not None else None,
            "current_distance": round(state.current_distance, 5) if state.current_distance is not None else None,
            "total_move_distance": round(state.total_move_distance, 1),
            "is_measuring": state.measuring,
            "measure_mode": state.config["measure_mode"],
            "angle_step": state.config["angle_step"],
            "current_round": state.system_status.split('(')[-1].split('/')[0] if '(' in state.system_status else None
        })

@app.route('/api/config', methods=['GET'])
def get_config():
    with state.lock:
        return jsonify({"status": "success", "config": state.config})

@app.route('/api/export', methods=['POST'])
def export_csv():
    with state.lock:
        if not state.csv_file or not os.path.exists(state.csv_file):
            return jsonify({"status": "error", "message": "暂无测量数据"}), 400
        try:
            return send_file(
                state.csv_file,
                mimetype="text/csv",
                as_attachment=True,
                download_name=f"测量结果_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv"
            )
        except Exception as e:
            return jsonify({"status": "error", "message": f"导出失败: {str(e)}"}), 500

# -------------------------- 主函数 --------------------------
def main():
    print("=== 激光位移测量系统 ===")
    print(f"启动时间: {datetime.now().strftime('%Y-%m-%d %H:%M:%S')}")
    print(f"API服务地址: http://0.0.0.0:5000")
    print("------------------------")
    app.run(host='0.0.0.0', port=5000, debug=False, use_reloader=False)

if __name__ == "__main__":
    try:
        main()
    except KeyboardInterrupt:
        print("\n程序被用户终止")
        # 清理资源
        if state.serial_port and state.serial_port.is_open:
            state.serial_port.close()
        if state.motor:
            state.motor.close_motor()
        sys.exit(0)